#include "pointcloud_tools.h"
#include <iostream>
#include "lidar_tools.h"
#include "point_cloud.h"
#include "point_common.h"
#include <fstream>
#include <cassert>
using namespace  pointcloud_tools;

int main(int argc, char** argv) {
    // Offline mode
    std::string lidar_ip = "192.168.10.108";
    int ret = 0;
    //Step 1 : Specify a pcap file, which contain the lidar's pointcloud packet.
    std::string pcap_filename = "../data/lidar_sample/two_chair_video.pcap"; 
    //Specify a calibration file, which contain the lidar's calibration data.
    std::string cal_filename = "../data/lidar_sample/device.cal";
    //Step 2 : Specify the pcap file, calibtation file to play.
    //The ip address and udp destination port is used to filter the pcap file to play the special lidar data. 
    zvision::OfflinePointCloudProducer player(pcap_filename, cal_filename, lidar_ip, 2368);
    int size = 0;
    zvision::DeviceType type = zvision::LidarUnknown;
    if (ret = player.GetPointCloudInfo(size, type)){
        printf("OfflinePointCloudProducer GetPointCloudInfo failed, ret = %d.\n", ret);
        return 0;
    } else {
        if (0 == size){
            printf("No pointcloud found for lidar %s:%d.\n", lidar_ip.c_str(), 2368);
            return 0;
        } else {
            printf("Sample offline pointclouds, size = %d.\n", size);
        }
    }
    //Step 4: Iterate the pointcloud.
    std::vector<Points> points;
    std::vector<Point_ALL_INT> int_points;
    std::vector<size_t> voxel_map;
    std::vector<int32_t> points_vector;
    /*  ------------
        Example Code Start
        -----------  */
    Pointcloud_utils ptu;

    //Simulation Model
    ptu.ddrSize = 8 * 8 * 1024 * 1024 * sizeof(int); //256MB
    assert(ptu.Model_DDR_alloc("DDR_32MB.bin"));

    for (int i = 0; i < 1; ++i) {
        // Get the original point cloud data
        zvision::PointCloud pointcloud;
        if (ret = player.GetPointCloud(i, pointcloud)){
            printf("GetPointCloud error, frame number is %d, ret = %d.", i, ret);
            return 0;
        }    
        points.clear();
        for (auto const& p : pointcloud.points) {
            if ((p.x ==0 && p.y ==0 && p.z == 0) || p.reflectivity == 0)
                continue;
            points.push_back(Points {p.x, p.y, p.z, p.reflectivity});
        }

        // save original point cloud data
        std::string pointcloud_filename = "pointcloud_" + std::to_string(i) + ".txt";
        std::ofstream pointcloud_file(pointcloud_filename);
        if (!pointcloud_file.is_open()) {
            std::cerr << "CANNOT OPEN FILE: " << pointcloud_filename << std::endl;
            return 1;
        }
        for (const auto& p : points) {
            pointcloud_file << p.X << "," << p.Y << "," << p.Z << "," << p.I << std::endl;
        }
    }
    

    return 0;

    // PointDisplay display;
}